	
	#include "Compass.h"
	
	
	//////////////////////////////////////////////////////
	//		EXTERNAL VARIABLES AS COMPASS OUTPUTs		//
	//////////////////////////////////////////////////////
	_iq MagneticHeading;

	void CompassHeading ( _iq roll, _iq pitch )   //Ptich and Roll inputs in Radians 
	{
		_iq MagneticXAxis,MagneticYAxis;
		_iq CosineRoll,SineRoll;
		_iq CosinePitch,SinePitch;
		
		CosineRoll  = _IQcos(roll); 
		SineRoll    = _IQsin(roll);
		CosinePitch = _IQcos(pitch);
		SinePitch   = _IQsin(pitch);
		
		#if SENSOR == SEN_10724
			/*
			Tilt compensated Magnetic field - X Axis
			MagneticXAxis = ( -magneticXAxis*cos(pitch) ) + 
	                    	( -magneticYAxis*sin(roll)*sin(pitch) ) + 
							(  magneticZAxis*cos(roll)*sin(pitch) );   */
			MagneticXAxis = _IQmpy(_IQ( (XAxisMagnetic-MAGN_X_OFFSET)),CosinePitch) + 
		                	_IQmpy(_IQ(-(YAxisMagnetic-MAGN_Y_OFFSET)),_IQmpy(SineRoll,SinePitch)) +
		                	_IQmpy(_IQ(-(ZAxisMagnetic-MAGN_Z_OFFSET)),_IQmpy(CosineRoll,SinePitch));
			/*
			Tilt compensated Magnetic field - Y Axis
	    	MagneticYAxis = ( -magneticYAxis*cos(roll) ) - ( magneticZAxis*sin(roll) ) */ 
			MagneticYAxis = _IQmpy(_IQ(-(YAxisMagnetic-MAGN_Y_OFFSET)),CosineRoll) -
		  					_IQmpy(_IQ(-(ZAxisMagnetic-MAGN_Z_OFFSET)),SineRoll);
		
			/*		MAGNETIC HEADiNG 	
		 	* Due to My 9DOF Sensor Stick Orientation, magnetic Axis signs 
		   	are inverted (only X and Y Axes).*/
			#if (IMU == FIRST_ORIENTATION)
				MagneticHeading = _IQatan2(-MagneticYAxis,MagneticXAxis);
				//MagneticHeading+= (_IQ(1.570796327));
				MagneticHeading+= (_IQ(MAGN_DECLINATION));
			#endif
			#if (IMU == SECOND_ORIENTATION)
				MagneticHeading = _IQatan2( MagneticYAxis,-MagneticXAxis);
				MagneticHeading-= (_IQ(1.570796327));
				MagneticHeading-= _IQ(MAGN_DECLINATION);
			#endif
		#endif
		
		#if SENSOR == SEN_10321
			/*
			Tilt compensated Magnetic field - X Axis
			MagneticXAxis = ( -magneticXAxis*cos(pitch) ) + 
	                   	 	( -magneticYAxis*sin(roll)*sin(pitch) ) + 
							(  magneticZAxis*cos(roll)*sin(pitch) );   */
			MagneticXAxis = _IQmpy(_IQ( XAxisMagnetic),CosinePitch) + 
		               		_IQmpy(_IQ(-YAxisMagnetic),_IQmpy(SineRoll,SinePitch)) +
		               		_IQmpy(_IQ(-ZAxisMagnetic),_IQmpy(CosineRoll,SinePitch));
			/*
			Tilt compensated Magnetic field - Y Axis
	   		MagneticYAxis = ( -magneticYAxis*cos(roll) ) - ( magneticZAxis*sin(roll) ) */ 
			MagneticYAxis = _IQmpy(_IQ(-YAxisMagnetic),CosineRoll) -
		  					_IQmpy(_IQ(-ZAxisMagnetic),SineRoll);
		
			/*		MAGNETIC HEADiNG 	
		 	* Due to My 9DOF Sensor Stick Orientation, magnetic Axis signs 
		   	are inverted (only X and Y Axes).*/
			#if (IMU == FIRST_ORIENTATION)
				MagneticHeading = _IQatan2(-MagneticYAxis, MagneticXAxis);
				MagneticHeading+= (-_IQ(MAGN_DECLINATION));
			#endif
			#if (IMU == SECOND_ORIENTATION)
				MagneticHeading = _IQatan2( MagneticYAxis,-MagneticXAxis);
				MagneticHeading+= _IQ(MAGN_DECLINATION);
			#endif
		#endif
	}
